/* soft clock implementation */

/* Copyright (C) 2009-2011 David Zanetti
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; version 2 of the License.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License along
 *  with this program; if not, write to the Free Software Foundation, Inc.,
 *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
 */


#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/atomic.h>
#include <stdio.h>
#include <avr/pgmspace.h>
#include <avr/eeprom.h>

#include "global.h"
#include <util/delay.h>

#include "rtc.h"
#include "serial.h"

/* internal clock control values */
uint16_t rtc_slew; /* ticks to slew by */
uint16_t rtc_slew_period; /* portion of 1s to slew for */

/* FIXME, handle more than 32-bit seconds unixtime */

/* ISR to handle RTC overflow (1Hz output, effectively) */
ISR(RTC_OVF_vect) {
	/* maintain uptime */
	global_counters.uptime++;
	rtc_s++;
	if (rtc_s == 0) {
		rtc_epoch++;
	}
	/* indicate to main loop 1Hz ticker has fired */
	flag_clock |= FLAG_CLOCK_TICK;
	PORTC.OUTTGL = PIN0_bm;
}

ISR(RTC_COMP_vect) {
	static uint8_t last = 0;
	/* insert one additional tick */
	if (last == 0) {
	 //while (RTC.STATUS & RTC_SYNCBUSY_bm);
	 /* 16387 is actually when we'll be by the time this interrupt is executing */
	 RTC.CNT = 16388; /* we got called at 16383, plus some sync ticks */
	 last = 3;
	}
	last--;
}

/* initalise the clock */
void rtc_init(void) {

#ifdef DEBUG_RTC
	serial0_tx_PGM(PSTR("rtc: init\r\n"));
#endif

	rtc_s = 0;
	rtc_epoch = 0;

	/* start internal 32.768kHz RC osc */
	OSC.CTRL |= OSC_RC32KEN_bm;

#ifdef DEBUG_RTC
	serial0_tx_PGM(PSTR("rtc: waiting for tosc ready\r\n"));
#endif 

	/* wait for clock ready */
	while (!(OSC.STATUS & OSC_RC32KRDY_bm));

	/* switch RTC clock source to 32.768kHz from TOSC, then enable it as the source */
	CLK.RTCCTRL = (CLK.RTCCTRL & ~(CLK_RTCSRC_gm)) | CLK_RTCSRC_RCOSC_gc;
	CLK.RTCCTRL |= CLK_RTCEN_bm;

	/* wait for RTC sync to complete */
#ifdef DEBUG_RTC
	serial0_tx_PGM(PSTR("rtc: waiting for RTC domain sync\r\n"));
#endif
	while (RTC.STATUS & RTC_SYNCBUSY_bm);
	/* initalise the RTC as zero */
	RTC.CNT = 0;

#ifdef DEBUG_RTC
	serial0_tx_PGM(PSTR("rtc: waiting for RTC domain sync\r\n"));
#endif
	while (RTC.STATUS & RTC_SYNCBUSY_bm);
	/* set period to 1023, since internal 32kHz RC osc is divided down to 1024Hz */
	RTC.PER = 1023;

#ifdef DEBUG_RTC
	//serial0_tx_PGM(PSTR("rtc: waiting for RTC domain sync\r\n"));
#endif
	//while (RTC.STATUS & RTC_SYNCBUSY_bm);
	/* set compare of RTC to 16383, half way up each second */
	//RTC.COMP = 16383;

#ifdef DEBUG_RTC
	serial0_tx_PGM(PSTR("rtc: waiting for RTC domain sync\r\n"));
#endif
	while (RTC.STATUS & RTC_SYNCBUSY_bm);
	/* start clock running, 1:1 prescaler */
	RTC.CTRL = RTC_PRESCALER_DIV1_gc;

	/* select low-level interrupt for overflows. It's only the 1Hz tick for the main loop, so low prio */
	RTC.INTCTRL = (RTC.INTCTRL & ~(RTC_OVFINTLVL_gm)) | RTC_OVFINTLVL_LO_gc;
	/* select low-level interrupt for compares, it's used to correct the crystal */
	//RTC.INTCTRL = (RTC.INTCTRL & ~(RTC_COMPINTLVL_gm)) | RTC_COMPINTLVL_LO_gc;

#ifdef DEBUG_RTC
	serial0_tx_PGM(PSTR("rtc: running\r\n"));
#endif

}

#if 0
/* align the clock to the global data provided about it's offset */
void clock_align(void) {
	long ticks;

#ifdef DEBUG_RTC
	serial0_tx_PGM(PSTR("GPS:     "));
	serial0_tx_dec(rtc_s_gps);
	serial0_tx_cr();
	serial0_tx_PGM(PSTR("Local:   "));
	serial0_tx_dec(rtc_s_pps);
	serial0_tx_cout(' ');
	serial0_tx_dec(rtc_ms_pps);
	serial0_tx_cout(' ');
	serial0_tx_dec(clock_us_pps / 20);
	serial0_tx_cr();
#endif

	/* okay, so if we have a large gap, then we might as well just
	 * hard reset the clock */
	ticks = rtc_s_gps - rtc_s_pps;
	if (ticks < -10 || ticks > 10) {
		/* yeah just hard reset it */
		clear_flag(flag_clock,FLAG_CLOCK_SYNC); /* just in case */
		/* ensure this is done clean */
		ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
			rtc_s = rtc_s_gps - 2;
		}
		serial0_tx_PGM(PSTR("rtc: reset\r\n"));
		return;
	}

	/* okay, we have less than 10 seconds shift, reduce the offset down
    to the total number of CPU ticks required */
	ticks = (rtc_s_gps - rtc_s_pps) * (uint32_t) ticklen * 1000L;
	ticks -= (rtc_ms_pps * (uint32_t) ticklen);
	ticks -= clock_us_pps;

#ifdef DEBUG_RTC
	serial0_tx_PGM(PSTR("rtc: ticks required "));
	if (ticks > 0) {
		serial0_tx_dec(ticks);
	} else {
		serial0_tx_cout('-');
		serial0_tx_dec(-ticks);
	}
	serial0_tx_cr();
#endif

	if (ticks < -20000 || ticks > 20000) {
		if (flag_clock & FLAG_CLOCK_SYNC) {
			serial0_tx_PGM(PSTR("rtc: lost sync\r\n"));
		}
		clear_flag(flag_clock,FLAG_CLOCK_SYNC); /* we do not have sync at the moment */
	}

	/* okay, we now know how many ticks we have to make up */

	if (ticks < -2000000 || ticks > 2000000) {
		/* adjust ticklen by about 10% to shift */
		ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
			rtc_slew_period = 1001; /* run for at least 1 second */
			if (ticks > 0) {
				//   clock_slew = 1000;
				rtc_slew = (ticklen - (ticklen / 10));
			} else {
				rtc_slew = (ticklen + (ticklen / 10));
			}
			flag_clock |= FLAG_CLOCK_SLEW; /* safe, interrupts are off */
		}
		return;
	}

	if (ticks < -200000 || ticks > 200000) {
		/* we're still quite far off, but let's slow down how much
     shift we're asking for */
		ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
			rtc_slew_period = 250; /* only half a second now */
			if (ticks > 0) {
				rtc_slew = (ticklen - (ticklen / 100));
			} else {
				rtc_slew = (ticklen + (ticklen / 100));
			}
			flag_clock |= FLAG_CLOCK_SLEW;
		}
		return;
	}

	if (ticks < -20000 || ticks > 20000) {
		ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
			rtc_slew_period = 250; /* only 100ms of each second will be slewed */
			if (ticks > 0) {
				rtc_slew = (ticklen - (ticklen / 1000));
			} else {
				rtc_slew = (ticklen + (ticklen / 1000));
			}
			flag_clock |= FLAG_CLOCK_SLEW;
		}
		return;
	}

	/* run free if we're less than 1k ticks away */
	if (ticks >= -2000 && ticks <= 2000) {
		if (!(flag_clock & FLAG_CLOCK_SYNC)) {
			serial0_tx_PGM(PSTR("rtc: sync\r\n"));
		}
		rtc_update_s = rtc_s;
		rtc_update_ms = clock_frac;
		clock_update_us = TCNT1;
		ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
			rtc_slew_period = 0;
			flag_clock &= ~(FLAG_CLOCK_SLEW);
			flag_clock |= FLAG_CLOCK_SYNC;
		}
		return;
	}

	/* if we are slewing, then try to get down to 1k */
	if (flag_clock & FLAG_CLOCK_SLEW) {
		/* we're between 10k and 1k ticks, if we're more than 2k, slew it down */
		if (ticks < -1000 || ticks > 1000) {
			ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
				rtc_slew_period = 10; /* only 50ms of each second will be slewed */
				if (ticks > 0) {
					rtc_slew = ticklen - 93;
				} else {
					rtc_slew = ticklen + 93;
				}
				flag_clock |= FLAG_CLOCK_SLEW;
			}
			return;
		}
	}

	/* we're not slewing, let it pick up to 2k before slewing again */
	if (ticks < -3000 || ticks > 3000) {
		ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
			rtc_slew_period = 10; /* only 50ms of each second will be slewed */
			if (ticks > 0) {
				rtc_slew = ticklen - 92;
			} else {
				rtc_slew = ticklen + 93;
			}
			flag_clock |= FLAG_CLOCK_SLEW;
		}
		//  serial0_tx_PGM(PSTR("-3k to 3k\r\n"));
		return;
	}

	/* run free, but with flags instact */
	ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
		rtc_slew_period = 0;
	}

	return;

}

#endif
